/**
 ******************************************************************************
 * @file    motor_driver.c
 * @brief   TB6612FNG 电机驱动库源文件
 * @author  XiFeng (Adapted for TB6612)
 * @date    2025-07-07
 ******************************************************************************
 * @attention
 *
 * 本库专为TB6612FNG电机驱动芯片设计
 * - 使用1个PWM引脚控制速度
 * - 使用2个GPIO引脚(IN1, IN2)控制方向
 * - STBY引脚需要在外部单独置为高电平来使能芯片
 *
 ******************************************************************************
 */

/* Includes ------------------------------------------------------------------*/
#include "motor_driver.h" // 您需要修改 motor_driver.h 文件以匹配新的结构体

/* Private function prototypes -----------------------------------------------*/
static uint32_t Speed_To_PWM(int8_t speed);
static int8_t Motor_ValidateParams(Motor_t *motor);



/* Exported functions --------------------------------------------------------*/

/**
 * @brief 创建电机实体 (适配TB6612)
 * @param motor: 电机实体指针
 * @param htim: PWM定时器句柄
 * @param channel: PWM通道
 * @param in1_port, in1_pin: 方向控制引脚1
 * @param in2_port, in2_pin: 方向控制引脚2
 * @param reverse: 是否反转电机定义 (1为是, 0为否)
 * @retval 0: 成功, -1: 失败
 */
int8_t Motor_Create(Motor_t *motor,
                    TIM_HandleTypeDef *htim,
                    uint32_t channel,
                    GPIO_TypeDef *in1_port,
                    uint16_t in1_pin,
                    GPIO_TypeDef *in2_port,
                    uint16_t in2_pin,
                    uint8_t reverse)
{
    // 参数检查
    if (motor == NULL || htim == NULL || in1_port == NULL || in2_port == NULL)
    {
        return -1;
    }

    if (channel != TIM_CHANNEL_1 && channel != TIM_CHANNEL_2 &&
        channel != TIM_CHANNEL_3 && channel != TIM_CHANNEL_4)
    {
        return -1;
    }

    // 初始化硬件配置 (***修改点***)
    motor->hw.htim = htim;
    motor->hw.channel = channel;
    motor->hw.in1_port = in1_port;
    motor->hw.in1_pin = in1_pin;
    motor->hw.in2_port = in2_port;
    motor->hw.in2_pin = in2_pin;

    // 初始化电机状态
    motor->speed = 0;
    motor->state = MOTOR_STATE_STOP;
    motor->enable = 1;
    motor->reverse = reverse;

    // 启动对应的PWM通道
    HAL_TIM_PWM_Start(motor->hw.htim, motor->hw.channel);
    
    // 设置初始状态：停止 (IN1=L, IN2=L, PWM=0)
    HAL_GPIO_WritePin(motor->hw.in1_port, motor->hw.in1_pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(motor->hw.in2_port, motor->hw.in2_pin, GPIO_PIN_RESET);
    __HAL_TIM_SET_COMPARE(motor->hw.htim, motor->hw.channel, 0);

    return 0;
}

/**
 * @brief 设置电机速度 (适配TB6612)
 */
int8_t Motor_SetSpeed(Motor_t *motor, int8_t speed)
{
    // 参数和使能检查
    if (Motor_ValidateParams(motor) != 0 || !motor->enable)
    {
        return -1;
    }

    // 速度范围检查
    if (speed < MOTOR_SPEED_MIN || speed > MOTOR_SPEED_MAX)
    {
        return -1;
    }

    // 保存速度值
    motor->speed = speed;

    // 如果电机需要反向，则将速度指令取反
    int16_t actual_speed = motor->reverse ? -speed : speed;

    // 根据速度设置方向和PWM (***核心修改点***)
    if (actual_speed > 0) 
    {
        // 正转
        HAL_GPIO_WritePin(motor->hw.in1_port, motor->hw.in1_pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(motor->hw.in2_port, motor->hw.in2_pin, GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(motor->hw.htim, motor->hw.channel, Speed_To_PWM(actual_speed));
        motor->state = MOTOR_STATE_FORWARD;
    } 
    else if (actual_speed < 0)
    {
        // 反转
        HAL_GPIO_WritePin(motor->hw.in1_port, motor->hw.in1_pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(motor->hw.in2_port, motor->hw.in2_pin, GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(motor->hw.htim, motor->hw.channel, Speed_To_PWM(actual_speed));
        motor->state = MOTOR_STATE_BACKWARD;
    }
    else
    {
        // 停止 (滑行/Coast)
        HAL_GPIO_WritePin(motor->hw.in1_port, motor->hw.in1_pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(motor->hw.in2_port, motor->hw.in2_pin, GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(motor->hw.htim, motor->hw.channel, 0);
        motor->state = MOTOR_STATE_STOP;
    }

    return 0;
}

/**
 * @brief 停止电机 (滑行模式)
 */
int8_t Motor_Stop(Motor_t *motor)
{
    if (Motor_ValidateParams(motor) != 0)
    {
        return -1;
    }

    // TB6612 滑行停止: IN1=L, IN2=L, PWM=0
    HAL_GPIO_WritePin(motor->hw.in1_port, motor->hw.in1_pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(motor->hw.in2_port, motor->hw.in2_pin, GPIO_PIN_RESET);
    __HAL_TIM_SET_COMPARE(motor->hw.htim, motor->hw.channel, 0);

    motor->speed = 0;
    motor->state = MOTOR_STATE_STOP;

    return 0;
}

/**
 * @brief 刹车电机 (新增函数)
 */
int8_t Motor_Brake(Motor_t *motor)
{
    if (Motor_ValidateParams(motor) != 0)
    {
        return -1;
    }

    // TB6612 刹车: IN1=H, IN2=H
    HAL_GPIO_WritePin(motor->hw.in1_port, motor->hw.in1_pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(motor->hw.in2_port, motor->hw.in2_pin, GPIO_PIN_SET);
    __HAL_TIM_SET_COMPARE(motor->hw.htim, motor->hw.channel, 0); // PWM可以为任意值，设为0即可

    motor->speed = 0;
    motor->state = MOTOR_STATE_BRAKE; // 建议在 motor_driver.h 中增加此状态

    return 0;
}


/**
 * @brief 使能/失能电机 (软件层)
 */
int8_t Motor_Enable(Motor_t *motor, uint8_t enable)
{
    if (Motor_ValidateParams(motor) != 0)
    {
        return -1;
    }

    motor->enable = enable;

    // 如果失能，立即停止电机
    if (!enable)
    {
        Motor_Stop(motor);
    }

    return 0;
}


/*
 * 以下是未改变或只需微调的函数
 */

/**
 * @brief 获取电机状态
 */
MotorState_t Motor_GetState(Motor_t *motor)
{
    if (Motor_ValidateParams(motor) != 0)
    {
        return MOTOR_STATE_ERROR;
    }
    return motor->state;
}

/**
 * @brief 将速度值(-100到100)转换为PWM比较值(0到MOTOR_PWM_PERIOD)
 */
static uint32_t Speed_To_PWM(int8_t speed)
{
    uint8_t abs_speed = (speed > 0) ? speed : -speed;

    if (abs_speed == 0) return 0;
    
    // 转换为PWM值：速度百分比 * PWM周期
    uint32_t pwm_value = (uint32_t)abs_speed * MOTOR_PWM_PERIOD / 100;

    // PWM阈值提升逻辑：如果计算出的PWM小于阈值，提升到阈值
    if (pwm_value > 0 && pwm_value < MOTOR_MIN_PWM_THRESHOLD)
    {
        pwm_value = MOTOR_MIN_PWM_THRESHOLD;
    }

    // 确保不超过最大值
    if (pwm_value > MOTOR_PWM_PERIOD)
    {
        pwm_value = MOTOR_PWM_PERIOD;
    }

    return pwm_value;
}

/**
 * @brief 验证电机参数有效性
 */
static int8_t Motor_ValidateParams(Motor_t *motor)
{
    if (motor == NULL ||
        motor->hw.htim == NULL ||
        motor->hw.in1_port == NULL || // ***修改点***
        motor->hw.in2_port == NULL)   // ***修改点***
    {
        return -1;
    }

    return 0;
}
